AD  660640 


NWC  TP  4420 


1 


NONLINEAR  ESTIMATION  TECHNIQUES 

by 

S.  R.  Nea! 

Systems  Development  Department 


■if 
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presented. 
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Nonlinear  Estimation  Techniques 


Summary 

This  paper  considers  a  discrete,  real  time  nonlinear  estimation 
problem  using  a  least  squares  criterion.  A  sequential  algorithm  is 
derived  which  allows  consideration  of  second  order  nonlinearities  in 
system  measurements.  Alternate  nonlinear  estimation  techniques 
are  discussed  and  examples  are  given  which  compare  the  various 
estimation  algorithms. 


Introduction 

A  sequential  solution  to  the  optimal  linear  (gaussian)  estimation 
problem  is  well  known  [3,  4,  5]*  and  has  been  used  extensively.  The 
problem  of  real  time  (i.  e.  sequential)  nonlinear  filtering  has  received 
considerable  attention  with  varying  degrees  of  success  [l,  2,  6,  7,  8]. 
The  typical  approach  to  the  sequential  nonlinear  estimation  problem 
consists  of  a  local  linearization  cf  the  various  nonlinearities  together 
with  an  application  of  the  linear  theory  [1,  8].  Sridhar  and  Detchmcndy 
[2]  have  considered  a  continuous  nonlinear  filtering  problem  from  a 
least  square  point  of  view  with  results  which  alio  /  second  order  non- 
linearities  in  the  observations  to  be  considered  in  the  computation  of 
the  filter  gain  matrix. 

This  paper  first  considers  a  discrete  nonlinear  estimation  problem 
using  a  least  squares  criterion.  A  sequential  algorithm  is  derived  which 
allows  consideration  of  second  order  nonlinearities  in  system  measure¬ 
ments.  The  Sridhar-Detchmendy  filter  [2]  is  then  obtained  by  a  straight¬ 
forward  limiting  argument. 

In  order  to  test  the  utility  of  the  derived  algorithm,  the  more  popular 
approaches  to  nonlinear  estimation  are  discussed  and  examples  are  given 
which  compare  the  various  estimation  algorithms. 


*  Numbers  enclosed  in  brackets  designate  references. 
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1.  Problem  Formulation  and  Solution: 


Consider  a  system  of  the  form 

Xfc+i  =  f(xk,  uk,  k)  +  Akwk 

(1) 

y*  =  h(xk,  k)  +  vk 

(2) 

where  x,  y,  u,  v,  w  are  column  vectors  and  h,  f  represent  vector 
valued  functions  of  their  arguments.  The  vector  u  represents  a 
deterministic  system  input,  w  represents  a  stochastic  system  input 
distributed  into  the  system  by  the  matrix  L  while  v  represents  meas¬ 
urement  error. 

Based  on  the  measurements  yi,  ....  y»,  a  sequential  estimate 
of  x,  is  desired.  In  order  to  obtain  a  solution,  this  problem  is 
imbedded  in  a  somewhat  larger  problem,  namely  obtain  those  estimates 


{wi.  J 


which  minimize 


Ja  =  ll*o,a  -  o'* 

+  )^(lly»n  -  Mxtn,..  i+  l)ll^tn-x+  llw».*HQrI)  <3> 

subject  to  the  constraints 

&IM,  a  =  f(*t,  a,  ut*  i)  +  ^lwl,a»  1  =  0,  ....  n  -  1  (4) 


When  the  random  variables  wj,  v»,  xo  **e  independent  and  gauss- 
ian  with  covariance  matrices  given  respectively  by  Q|,  R»,  Po,  o.  then 
(3)2  results  from  a  maximum  likelihood  criterion  [!}.  However,  in 


^Numbers  enclosed  by  parentheses  (  )  designate  equations. 
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a  practical  situation  very  little  is  known  about  the  statistics  of  the 
several  variables.  At  best,  estimates  of  the  first  and  second 
moments  of  each  component  of  the  vector  random  variables  are 
available.  In  such  a  situation,  (3)  may  be  considered  as  simply  a 
"least  squares"  criterion.  The  positive  definite  matrices  Po,o>  Ri, 
Qi  can  be  diagonal  with  estimates  of  the  second  moments  placed 
at  the  appropriate  coordinate.  To  simplify  notation,  the  arguments 
Uk,  k  will  normally  be  suppressed  in  the  sequel. 

To  obtain  the  desired  estimates,  introduce  multiplier  vectors 
{2Xi,  B  }  and  form  the  functional 


C#  -  Jt  +  2  E  Xi  a  tx1+i,  •  -  f(xi,  b )  -  AjWj,  a  ] 

1=0 

Performing  the  required  differentiation  and  equating  the  various 
derivatives  to  zero  yields  the  following  two  point  boundary  value 
problem: 


A 

'xl+l>  a 

=  f(xt,  .)  +  AiQ.AlXi,.,  i  =  0,  . .  .  ,  n  -  1 

(5) 

Xi-1,  a 

=  f!(xi,a)Xi,  ,  +  hI(xlt,)R1'1[yl  -  h(x,,  a)  3, 

i  =  1 ,  .  . .  ,  n 

(6) 

A 

Xo,  a  = 

Xo,  O  +  Po,  of«  (Xo,  a)Xo,  a 

(7) 

Xa,  a  = 

0 

(8) 

An  approximate  solution  to  this  problem  can  be  obtained  by  induc¬ 
tion  on  n  and  i  [cf.  Appendix  A]  and  is  given  by1* 

*t,  •  =  *i,  i-i  +  Pi*  i  M*i,  i  ■  Xj,  i.i) *  n  2  1, 

i  =  0 . n  -  1  (9) 


Following  Cox  [l],  the  notation  A*  is  used  to  denote  an  approxi¬ 
mation  of  A.  The  superscript  (n)  is  used  to  emphasize  the  dependence 
of  the  various  variables  on  n. 
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where 


(10) 


Po,o  =  Po,o,  nil 

pl".  1  I1  •  P*“ -»  h  th«R*’1(y‘  '  h)]i*  .-1 1 

ni2,  i:l .  n-1  (11) 

P(,)  *  Pi,  0  =  (^0,  o)Po,  Of!  (*0,  o)  +  ^oQo  Ao .  n  4  2  *12* 

0 

P(1f1.1=  f,(x?-i,.-x)P<i-il  t-xfTx(xf-i,.-i)  +  ViQt-iAU, 

n  2  3,  i  =  2,  . 1  (15) 


The  approximate  solution  (9-13)  to  the  two  point  boundary  value 
problem  (5-8)  can  be  used  to  approximate  x»,  m  Lcf.  Appendix  J  . 
The  results  are  given  below  for  n  *  1: 


k*t  J.-X  =  f(x?-i,  m-l) 

kt, .  =  £,  -1  +  KmCy,  -  M&,  »-*)] 


K|  =  P»,|hi(x»,  »-i)Ri 

.-1 


P»« »  = 
P»,  i-l 


I1  '  P«.  *-11^  [h«Rl 

=  f| (&m-l,  »-l)P»-Xi  »- 


'l(y»- h)3**  a_J 
lfl(xj-l,»-t)  +  As-lQs-X^s-t 


(14) 

(15) 

(16) 

(17) 

(18) 


The  computations  are  initiated  with  xo,  o.  Po,  o. 


Cox  CO  and  others  have  considered  the  prediction  of  x»  based 
on  vi  .  ..  Yt-t  with  results  which  furnish  some  justification  for 
L  dUini.lJ  and  Dotation  u..d  in  (14).  Throu,houtth«  r.m.  ad.r 
of  this  paper.  (14)  shall  be  considered  as  one-sample  prediction 
while  (15)  shall  be  referred  to  as  filtering. 
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Assuming  that  certain  components  of  (17)  are  invertable,  alter¬ 
nate  forms  of  (16,  17)  con  be  obtained  [cf.  Appendix  A]  and  are 
given  below: 

K.  =  SBhT,(£*  .-x)[hl(^,..1)S#hT1(A*  ,.x)  +  R#rl  (16') 

S»  =  [I  -  Pn.-lL.]"1  P.,,.1  (16") 

P.,.  =  [I  -  K, h,  (x*,  ,_i)]  S,  (177) 

2  A 

L.  =  («„<»)>,  i„(n)  °  R‘"1  [y‘  *  (18) 

where  xt,  Xj  denote  respectively  the  i**1  and  j**1  components  of  the 
vector  x. 

In  the  last  form,  it  is  apparent  that  (16")  is  the  only  additional 
computing  require^,  ^.^.r  the  usual  linearization  oi  plant  dynamics  and 
observations.  Essentially,  (16")  represents  a  modification  of  the 
matrix  PB,,-X  based  on  an  appropriate  consideration  of  the  second 
order  nonlinearities  of  the  observations  h.  It  is  interesting  to  note  the 
close  resemblance  of  (15-17)  to  the  computing  algorithm  resulting  from 
the  minimization  of  ||y  -  h(x)||^-i  using  steepest  descent. 


2.  A  Continuous  Problem 

The  continuous  version  of  the  estimation  problem  under  considera¬ 
tion  has  been  formulated  and  solved  by  Sridhar  and  Detchmendy  [2]. 
Their  results  can  also  be  established  by  using  the  classical  method  of 
finite  differences  and  the  discrete  algorithm  (14-18). 

The  dynamics  and  observations  of  the  continuous  problem  are 
modeled  as 


A(t)  =  g(x,  u,  t)  +  G(t)w(t),  0  «  t  *  T  (21) 

y(t)  =  h(x,  t)  +  v(t)  ,  0  *  t  *  T  (22) 

where  the  various  variables  are  defined  as  above.  The  variables  x, 
y,  u,  w,  v,  G  are  continuous,  g  is  continuously  differentiable  with 
respect  to  x  and  h  is  twice  continuously  differentiable  with  respect 
to  x. 
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Given  y(t),  0  St  s  T,  estimates  x(t;T),  w(t;T)  of  x(t),  w(t)  respec¬ 
tively  are  desired  which  will  minimize 


J(T)  =  ||x{0;T)  -  £<0;0)||po-i 

+  j[T  (lly(t)  -  hU(t;T),t]||^-i(t)  +  II w(t;T) Hq-i ^t))dt  (23) 


subject  to  the  constraint 

x(t;T)  =  g[x,  u,t]  +  G(t)w(t;T),  0  st<T  (24) 

A  discrete  approximation  to  the  preceding  problem  is  obtained 
by  partitioning  the  interval  [0,  T]  into  n  equal  subintervals  [t1(  tt+i] 
each  of  length  At.  After  suppressing  the  arguments  u,  t,  the  results 
are 


xj+i  =  f (x|)  +  Atwi  i  =  0,  . . . ,  n  -  1 
y»  =  Mxt)  +  v,  i  =  1,  ....  n 


Ip  -i  +  ^ 

Fo  t=o 


...  n 

(26) 

<t+l,  i)ll  AtRm"x 

•  ■*1  2 

(27) 

it  i  =  0,  ...»  n  -  1 

(28) 

where 

f(X|)  =  x(  +  Aig(xj)  +  o(  At) 

A|  =  AtG(ti)  +  o(At) 

Applying  the  results  (14-1?)  to  the  discrete  problem  (25-30) 
obtains 
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**n>  *  It"  ^ =  g(**»*)  +  p^i.  »♦*  h«  *>3  R(T>'1  ty-»  -  + -^ 


Holding  T  =  n  At  constant,  letting  X*  =  x*(T)  =  x^  B>  P(T) 
taking  the  limit  as  At  “*  0  yields 


=  P«,  •  and 


A*  =  g(x*)  +  P(T)hT1(x*)R(T)‘1[y(T)  -  h(x*)]  (31) 


To  obtain  P(T),  use  (16-19,  A15)  and  (18,  29,  30)  to  obtain 
respectively 


PrU.rU  =  (I  -  K«+l  H»+i)[l  +  P»+i,.L.+  l(I  -  Pi+l,iLi*i)"  ]ps+l,  i  (32) 

P»+l,  •  =  Pi,  m  +  At[glP„.  +  Pi,  i  gx  +  GQGt]  +  o( At)  (33) 

where  H,+  i  =  hx[f(xt,  k)]. 

Substituting  (33)  into  (32) 


pint»>L B  glPm> ,  +  pt> B g;  +  gqgt 


sm 


+  Ip^.«(i5r)(1  *  p-*.*1-0’1  -  ) H,+x  I Pi*  * +  At 


which  implies  that 


where 


P  =  g,P  +  PgT,  +  GQGt  -  Phi  R  lh,P  +  PLP 


l m*«». 


(34) 


Combining  terms 

P  =  g»P  +  Pgl  +  p  ChlR'^y  -  h)]**?  +  CQGt  (35) 
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Equation  (31)  together  with  (34  or  35)  represents  an  approximate 
solution  to  the  continuous  filtering  problem.  Equations  (31,  35) 
represent  the  solution  as  derived  by  Sridhar  and  Detchmendy  [2], 

3.  Other  Nonlinear  Estimation  Techniques 

In  order  to  establish  a  basis  for  testing  the  utility  of  the  results 
cf  Section  1,  two  other  nonlinear  estimation  techniques  are  now  dis¬ 
cussed. 

First,  following  [8],  consider  a  system  of  the  form  (1,  2)  where 
the  m  components  of  h  are  linearly  independent  functions  of  the  m 
components  Hx  of  x.  Linearising  h  about  H&  yields  the  approximation 

y  =  h(Hx)  +  v  =  h(Hx)  +  J(Hx  -  HA)  +  v 

where  the  Jacobian  J  of  h  at  Hx  is  nonsingular.  Inverting  J  yields 
the  (transformed)  linear  observations 

y*  =  J"l[y  -  h(Hx)]  +  Hx  =  Hx  +  j'xv  .  (36) 

Using  the  linear  theory, 

x.,  .  =  x„  .-i  +  G.  ty?  -  Hx.,  .-iJ  (37) 

vhere  the  gain  sequence  {G.}  i«  computed  using  the  observability 
matrix  H  and  the  measurement  error  covariance  matrix  J  lR(J  l)T. 
Substituting  (36)  into  (37)  yields 

x»,  ■  =  x.t  .-i  +  G.  J.  l[y.  -  h(x.,  ..x)]  (38) 

One  interesting  aspect  of  (38)  is  that  the  gain  matrix 

K,  =  G.  J.  1 

is  factored  into  a  product  of  a  smoothing  component  G.  and  a  linear¬ 
ising  compouent  J.  *.  Moreover  nominal  values  can  often  be  chosen 
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for  J  1R(j"1)T  and  f*  thus  allowing  the  gain  sequence  {G»}  to  be  precom¬ 
puted  and  stored  in  computer  memory  for  real  time  data  processing. 


Another  approach  which  is  often  successful  consists  of  using  a 
one-to-one  nonlinear  transformation  c?~to  map  the  original  nonlinear 
problem  (1,  2)  into  a  space  in  which  the  transformed  problem  is 
linear,  i.  e. 


cT(x,+i)  =  $»+i, 


(39) 


o*~(y»)  =  M.c^x.)  +  v»  (40) 

The  statistics  of  the  measurement  error  (v,)  in  (40)  are  usually 
quite  complicated  and  very  nongaussian.  However,  a  direct  application 
of  the  linear  theory  (rationalized  by  the  least  square  interpretation  of 
Section  1)  often  provides  excellent  estimates <^(x)  of  oHx).  Moreover, 
the  estimate 

x  1  uAx)]  (4i) 


may  also  be  quite  good.  Needless  to  say,  the  resulting  estimates  are 
not  necessarily  optimum. 

In  the  sequel,  nonlinear  estimation  using  the  standard  linearization 
of  system  dynamics  and  observations,  i.e.  using  (14-18)  with  L  =  0.  will 
be  referred  to  as  Type  I  estimation,  using  (14-18)  as  derived  will  be 
called  Type  II.  .he  approach  of  [8]  as  described  above  is  Type  III  while- 
the  last  approach  just  described  isoPestimation. 


4.  Examples 

Several  controlled  experiments  have  been  conducted  in  order  to 
test  the  utility  of  the  various  discrete  nonlinear  estimation  techniques. 
Two  of  the  experiments  will  be  described  below.  (In  the  following  two 
examples,  lower  case  alphabetic  characters  will  denote  scalars.) 

First  consider  a  discrete  form  of  example  2  of  [2],  The  system 
equations  are 


xx  -  2,  x\  =  0,  &x  -  2,  T  -  0.1 
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and  for  n  ^  1, 


x#+i  =  x»  +  Txt  (42) 

xa+i  =  (1  -  3T)x,  -  2Tx,  -  TaBxB3  +  5T  sin  (nT)  (43) 

aB+i  =  e  &B  (44) 


In  order  to  provide  a  significant  comparison  of  the  estimation  algo¬ 
rithms  under  consideration,  output  observations  with  significant 
nonlinearities  should  be  considered.  (This  is  a  departure  from  the 
examples  considered  in  [2].)  Consequently,  the  output  observations 
for  this  example  are  modeled  as 


yB  =  2  sin  (^i)  +  vB  (45) 

which  simulates  severe  saturation  near  the  maximum  position  ampli¬ 
tude.  Since  the  dynamic  range  of  the  position  ranges  from  about  -1.5 
to  +1.5  after  initiation,  the  observations  are  not  one-to-one.  Hence 
it  was  not  obvious  in  advance  that  the  system  (42-45)  was  observable. 

Neither  the  Type  III  algorithm  nor  the  pFl transformation  technique 
seem  suited  for  this  example.  The  Type  I,  II  estimation  equations  are 

xx.o  =  0,  $i,o  =  0,  ax  =  2  . 


and  for  n  *  1 , 


xB,  B  =  xB,  B-i+  kn(n)  [  yB  -  2  sin  ] 

x,,B  =  £»..-i+  kgi(n)  |  y,  -  2  sin  ( ***| )  | 

A  A  ,  _<> 

xB»i,  •  -  *»,  ■  +  Txb,  b 


\ 
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x1+i,  m  =  (1  -  3T&. ,  -  2t£.,  .  -  Ta,xi>  ,*  +  5T  sin  (nT) 


a»+i  -  e  aB 


The  gain  matrix  ^ixj  is  obtained  by  numerically  solving  ( 167,  16*, 
17',  18,  19)  with 


-T(2  +  3  anxa,  sa)  (1-3T) 


*.-i;  :\ 


h*  - 


17COS  (— £ 


“)• 


.  Q  =  o,  R  =  CT2  =  E(v.2) 


For  Type  I  estimation,  L  -  0  while  for  Type  II, 


•  It  1  ffxB.  n-l\ 

tu  T  1  2  / 


y»  -  sin 


(A“)l 


is  the  only  nonzero  component  of  L. 

With  (T  =  0.1,  typical  results  for  position  estimation  are  illustrated 
in  Figure  1.  The  results  for  velocity  estimation  are  similar.  It  seems 
that  Type  I  estimation  has  better  initial  estimates  while  Type  II  is 
better  after  ibout  10  samples  have  been  processed.  However,  there 
is  little  difference  after  processing  30  samples.  These  results  were 
substantiated  by  a  50  run  Monte  Carlo  experiment.  The  rms  position 
estimation  errors  vs.  time  are  given  in  Figure  2. 

Since  the  examples  given  in  (.2]  only  used  measurement  error  of 
approximately  the  same  magnitude  as  above,  it  seemed  worthwhile  to 
consider  lower  signal  to  noise  ratios.  As  a  was  increased  a  rather 
startling  result  occurred  for  O  near  1  (S/N  0  db).  For  this  lower 
signal  to  noise  ratio,  the  Type  U  algorithm  usually  performed  best 
whenever  it  would  work,  however,  it  would  occasionally  become  unstable 
during  the  first  few  samples  of  tracking.  Instability  was  never  observed 
in  the  Type  1  algorithm.  Moreover,  the  Type  I  algorithm  furnished  good 
estimates  of  both  position  and  velocity  for  all  values  of  0.  In  fact  the 
rms  errors  for  the  Type  1  algorithm  with  Q  -  1  differed  very  little  from 
the  case  <X  0.1.  The  estimation  errors  seem  almost  entirely  due  to  a 
dynamic  bias  resulting  from  poor  initiation. 

1  1 


2 

i 

j 

As  a  second  example,  consider  a  two  dimension  tracking  problem.  f 

Target  range  and  azimuth  are  measured  every  second  with  an  accuracy  \ 

of  200  yards  and  two  milliradians  respectively.  (Notice  that  no  statis¬ 
tical  information  concerning  the  errors  is  available.) 

The  target  dynamics  are  modeled  in  a  Cartesian  reference  frame 

as 

T  2  i 

x,+  i  =  xB  +  T,x#  +  -y-  wla 
x**i  =  x*  +  T,  wi» 

i 

with  similar  equations  for  the  y-axis.  The  sequence  {w^,}  represents 
random  acceleration  components  along  the  x-axis.  The  output  observa¬ 
tions  are 

r  =  (x3  +  y3)1^2  +  vi  0  =  Arctan  (^)  +  v®  • 

All  four  approaches  discussed  above  can  be  used  for  this  example. 
Anc^-transformation  for  this  example  consists  of  performing  a  polar 
to  Cartesian  coordinate  transformation  on  the  measurements  R,  0  and 
tracking  in  the  x-y  plane  ignoring  the  introduction  of  correlated  errors 
on  the  synthetic  measurements.  The  resulting  x-axis  estimation 
equations  are 

A  ^ 

XX,  o  =  Xx,  o  =  0  * 

A  A 

X,,  *  = 

£»  A 

Xt,  •  -  x*(  l-l  +  T  *»• 

A  A  ^  _ 

x«fi,t  -  *»,  i  +  T*xb>» 

*  A 

x*»  i,  t  -  x*,  a 

a  A 

Cb  =  r,  cos  0g  -  x*t  i-i 


U 


with  analogous  equations  for  the  y-axis.  (This  is  a  nonstationary  form 
of  the  familiar  CK-/3  tracking  filter. )  This  filter  tracks  very  well  using 
an  initial  segment  of  the  gain  sequence  computed  with 


which  allows  the  gains  to  be  precomputed.  Estimates  of  range  and 
azimuth  are  furnished  by 


r  =  (£a  +  $a)1/a 
6  =  Arctan  (y /£)  . 

Type  Ill  estimation  is  performed  exactly  as  above  except  that 
xi,i  =  ri  cos  Qx  ,  xi,  l  =  0  , 


-  r.)  -$.,.-x 


(6.  -  0.) 


A 


.A  2 

-  (*», ■-! 


A 

y*,»- 


a.Ua 
l  ) 


A 

0,  -  Arctan 


For  Type  I,  U  estimation 

A  A  A  A 

+  kxx(n)(r,  -  rj  +  kia(n)(0,  -  0,) 

»  -  x»,  »-x  +  k*x(n)(r,  -  rj  +  k^n)^,  -  0») 

Attempting  to  avoid  numerical  problems  in  the  computation  of  the  gain 
matrix 

K.  =  (k„(n)) 
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the  position  units  are  taken  as  [hundred  yd]  while  [milliradian]  is  used 
for  the  angle  unit.  It  follows  that 


To  assure  nonzero  steady  state  gains, 


and  for  good  transient  response, 

Pi,o  =  104I  . 

For  Type  II  estimation,  the  nonzero  components  of  L  are 


For  this  example,  the  Type  I,  II  algorithms  respond  very  poorly 
unless  initiated  with  good  initial  estimates  o t  target  position  and 
velocity.  Both  Type  IQ  estimation  and  the  ^transform  filter  always 
performed  very  well  for  both  position  and  velocity  estimation.  Essen¬ 
tially  no  difference  between  the  two  was  observed  for  the  noise  levels 
described.  Typical  range  estimation  errors  for  the  four  filters  are 
given  in  F igure  3. 
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5.  Conclusions 


At  this  point,  it  is  standard  procedure  to  proclaim  the  usefulness 
of  derived  results  after  a  consideration  of  contrived  examples.  While 
such  conclusions  often  "look  good  on  paper,  "  it  is  very  frustrating  to 
find  that  the  examples  are  unique  in  illustrating  the  utility  of  a  particu¬ 
lar  result. 

The  two  examples  given  above  were  designed  to  point  out  the  fact 
that  none  of  the  popular  approaches  to  nonlinear  estimation  represent 
a  universal  solution  to  nonlinear  estimation  problems.  Rather,  it 
appears  that  ingenuity  as  well  as  discretion  is  presently  required  in 
obtaining  practical  solutions  to  meaningful  nonlinear  estimation 
problems. 

A  few  empirical  results  which  appear  to  hold  in  general  are: 

a.  If  the  signal  to  noise  ratio  is  low  the  Type  II  algorithm  may 
be  unstable.  Of  all  the  approaches  considered,  the  Type  II  algorithm 
appears  the  most  sensitive  to  low  signal  to  noise  ratios. 

b.  If  the  initial  estimates  xi,o  based  on  aprion  knowledge  are 
poor,  both  Type  I  and  Type  II  algorithms  may  exhibit  poor  transient 
response.  These  algorithms  seem  especially  sensitive  to  initial 
errors  in  the  estimate  of  the  direction  of  the  initial  state  vector  xj. 

c.  If  either  Type  I  or  Type  II  estimation  is  used,  the  filter  gain 
matrix  normally  must  be  computed  on  line.  The  main  reason  being 
that  the  filter  gain  matrix  is  extremely  sensitive  to  the  value  of  the 
observability  matrix  h*.  Instability  will  usually  occur  when  attempts 
are  made  to  use  precomputed  gains.  However,  it  is  quite  possible 
to  formulate  the  Type  III  algorithm  and  an^3~transformation  in  such 

a  fashion  as  to  allow  the  use  of  precomputed  gains.  That  is,  the  last 
two  techniques  are  considerably  more  amenable  to  real  time  computa¬ 
tion. 


d.  When  applicable,  Type  III  estimation  or  appropriate  (^trans¬ 
formations  seem  to  enjoy  the  greatest  utility  of  the  four  techniques 
which  have  been  considered.  The  resulting  filters  exhibit  little 
sensitivity  to  initiation  errors  and  low  signal  to  noise  ratios. 

In  view  of  the  results  of  this  study  it  seems  that  the  results  of  C2] 
(or  of  Section  2)  should  be  exposed  to  the  problems  of  large  initiation 
errors  and  low  signal  to  noise  ratios.  Also,  the  performance  of  the 
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Sridhar-Detchmendy  filter  [2]  should  be  compared  with  algorithms 
which  can  be  considerably  less  complex  to  implement  (i.  e.  the  continu¬ 
ous  version  of  the  Type  1  and  Type  III  algorithms). 


Position 


RMS  Position  Error 


FIG.  2.  RMS  Position  Estimation  Errors  vs.  Sample  Number. 


Position  Estimation 


FIG.  3.  Range  Estimation  Errors  vs.  Time 


Appendix  A 


APPROXIMATE  SOLUTION  TO  THE  TWO  POINT 
BOUNDARY  VALUE  PROBLEM 

Consider  the  two  point  boundary  value  problem  derived  in  Section  1. 

*tn,»  =  f(xi,m)  +  AiQiAtXt,*,  i  =  0 . n-1  (Al) 

Xi-i,.  =  +  h«(xlf  1)Rt‘1[y1  -  h(£l>B)],  i  =  1 . n  (A2) 

X,,  ,  =  0  (A3) 

xo,  «  =  xo,  o  +  Po,  of*  (xo,  *)Xo,  •  (A4) 

Setting  n  =  1  and  linearising  f(xo,  i)  about  xo,o  yields 

x*  1  =  f(xo,  o)  +  f  *  (xo,  o)(xo,  i  -  *>,o)  +  AoQo^Xo.i 
Assuming 

U  (xo,  a)  =  f,  (xo,  o) 

and  using  (A2-A4)  yields  the  approximation 

x  =  f(xo,o)  +  Pj,,ohl(xi,  \)Rx*  [yv  -  h(xlt  x)] 

whe  re 

Pi,  o  =  f,{xo,o)Po,of*(xo,o>  +  AoQo 
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Linearizing  hl(xx,x)Ri  1  [yi  -  h(xx,i)]  about  x*,  o  s  f{xo,  o)  results  in 
a  second  approximation 


x*  i  =  xf,  o  +  Pi,  oh[(xi,  o)Ri  1  [yi  -  h(xi,  o)] 


T  -A* 


-ll 


■  A* 


+  Pi,  0  T-  [hi  Ri  1  (yi  -  h)]  A*  (xx,  i  -  x  i,  o) 

OX  0 


which  implies 


xx*  x  =  xx*  o  +  Pi,  x  hT,  (x*.  o)Ri”1Cyi  -  h(xx*  0)3 


where 


1 


From  (A2,  A4)  it  follows  that 

Xo,  1  =  Xo,  o  +  Po,  of[(xo,  o)h[{x*  x)Rx  1  [yx  -  h(x*i)] 

Now,  assuming  that  {a*  *„x ;  i  =  0 . n  -  l]  have  been  deter 

mined,  consider  the  following  proposition: 

For  n  ^  2, 


A  *  A  * 


X  • 


,T  .a* 


,  1 


x.,,-1  ♦  -  X,,,.!).  i  =  0 . n  -  1  (A  5) 


whe  re 


r> 

*0,0  -  Po,  o 


(A6) 


*«.  i 


I1  *  p<t^-*'^thU.'1(yi  -  h)]A*#  J  lpifM 


i  =  l . n  -  1  {A?) 


*  It  is  tacitly  assumed  that  the  appropriate  inverse  exists. 


P^o  =  Pi,0  =  fx(xo,o)Po,ofT*(xo,  o)  +  ^oQo^o  (A8) 

Plfl-l  =  .-l)P(A,  a-l)  +  Al-lQl-l^t-1. 

n  2  3,  i  =  2 . n  -  1  (A9) 

The  statement  A5  for  i  =  0  follows  immediately  from  A4.  Next, 
the  proposition  is  established  for  i  =  1,  n  *  2. 

Setting  i  =  1  in  A1  and  linearizing  f(xo,  a)  about  xo,  o  yields 
xf,  a  =  f(xo,o)  +  Pi,oXo,« 


which  implies 


where 


x*  K  =  xi,  a-l  +  Pi,  o(Ao.  a  -  Xo,  a-l) 


Xo,  a  =  fTa(Xi,a)\l,a  +  hl(xx,,)[yx  -  h(xlt,)] 

Xo,  .-1  =  fT,(xx,.-l)Xx,..l  +  hTa(Xx,.-i)[yi  -  b(Xx,,-i)] 

Linearizing  hT,(xx,  .)R»“X  In  -  *»(*i..)3  »bout  tf.a-l  «*d  assuming 
f.(xx,a)  =  f.  (xx*  a-i)  yields 

X*.  a  =  Xi,  a-l  +  Px,ofTa(xl.a-l)(Xl,a  “  Xx,a-l) 


+  i-  ChlRx'Nyi  -  b)3  A*  (xj,  ,  -  Xi,  ,.i) 

OX  Mf  1*1 


which  establishes  A5  for  i  =  1. 

Now,  from  Al, 

Xt.a  -  f(X|-i,a)  +  A|-lQl-iA»l  *»-!,•»  itl . 
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Linearizing  f(x1.lt  a)  about  Xi.j,  a_i  obtains 


xf,  B  =  f(xj'-i,  »-i)  +  M^-x.t-xH^-x, 


,  A* 


A 


i  A  # 


xf-1,  m-l)  +  ^1-xQl-l  &l-lXi-l,  a 


By  induction  on  i,  it  follows  that 

xf, »  =  f(xi_Xj  B_i)  +  (Xi-i,  •  -  Ai-x,m-i)  +  ^1-1  Qi-i  At-i  Ai-x,  B_i 


where 

=  f*(Xi-l,»-l)P<A,  i-xf*  (xf-x,  »-i)  +  ^i-xQi-x^i-x 

Noting  Al,  it  follows  that 

a*  a*  _(#)  \  \ 

xl,  a  -  xi,  ,_i  +  P1(  i_i  (At-i,  ,  -  A.j-1,  »_x) 

The  remaining  arguments  are  analogous  to  those  used  in  the  preceding 
case. 


The  approximate  solution  (a5-A9)  to  the  two  point  boundary  value 
problem  (A1-A4)  can  be  used  to  establish  a  computing  algorithm  for 
x?t  Setting  i  =  n-  linA5  yields 

A*  A*  -T  /  A*  .  . 

=  X«*li  i-i  T  “«»x,  *-l  t»  VX*-i,  a-x)Aa-x,  » 

A  A  ^ 

Linearizing  the  dynamics  f(x*.i,  »)  in  Al  about  x«_i,  and  using  the 
preceding  equation  implies 

x?,a  =  f(JUM.a-x)  ♦  P.,  -x  hT.  (x*  JR.'1  Ly,  -  h(£*  .)] 


whe  re 

P,,.-x  -  MCx,.-x)P..i..-xfT.(Cx,  ‘i.-iQi-i  ^«-x  (A10) 

Linearizing  h\  (xj1,  ,)R*”X  Cy*  -  h(x*  »)]  about  xT,  ,.x  *  f(x?.x,  *-i)  obtains 
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xi  »  =  Si*  +  P,f  .  hT,  (x*  ,-^Rn'1  [y.  -  h(£*  ,.i)] 


(All) 


with 


P, 


f 


A 


i  -  p.,i-i  it  ^h*R»  (y«  -  h)3** 

OX  x»(  !-l 


p»,  »-l 


(A12) 


Equations  (A10-A12)  constitute  a  computing  algorithm  for  x,,  t. 


Alternate  forms  of  (A12)  can  be  obtained  with  moderate  assump¬ 
tions.  Assuming  that  P«,.-i  is  nonsingular  allows  (A12)  to  be  expressed 
as 


P»,  •  =  {P.,  *-i  -  L»  +  h*  {xj,  a-i)R.  h*(x»tt-i)}  , 

L*  =  (Jt«(n)),  lu(n)  =.  R.'1  Cy.  -  h(x*  ,.x)] 

where  xx,  xx  denote  respectively  the  ith  and  jth  components  of  the 
vector  x. 

Using  the  matrix  identity 

[B~l  +  CR*1D]~1  =  B  -  BCtDBC  +  R]_1DB  (A13) 


and  assuming  that 

S.’1  =  P.,.-i’X  -  U 

is  invertable  yields 

P...  =  S,  -  S. hi C h« S. hi  +  R.3'Xh,S.  (A14) 

S|  =  Pa,  |.j  +  P§,  p _ X  Lib  Li  -  P»,  .-l  i^i]  P»,  »-l  (A  1  5) 
*  Cl  -  P.,.-iU3‘lP.,.-i  (A16) 
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It  now  follows  that 


Ka  =  PB,  a  hi  Rb"1  =  SB  h*  [hx  S#  h*  +  Rb]_1  (A17) 


which  implies  that 


P.,»  =  [I  -  K,h«]S, 


(A  18) 


i 
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